//
// Created by JYL on 2023-12-20.
//
#include "uart_task.h"
#include "balance_task.h"
#include "voltage_task.h"
#include "filter_task.h"
#include "usart.h"
#include "beep.h"
#include "mpu6050.h"
#include "pid.h"
#include "app.h"

extern osSemaphoreId_t myBinarySemuart_usbHandle;
extern osSemaphoreId_t myBinarySemuart_blueHandle;
extern osSemaphoreId_t myBinarySemuart_k210Handle;

extern osTimerId_t myTimer_carHandle;

u16 Flag_front,Flag_back,Flag_Left,Flag_Right,Flag_velocity=2,Target_Velocity=200; //蓝牙遥控相关的变量
_Bool PID_Send;

void UartRx_Task(void *argument)
{
    u8 rx_len=0;
    /* Infinite loop */
    for(;;)
    {//基本任务1，三个串口，发送的数据，能空闲中断发送回来，使用信号量触发
        if(osSemaphoreAcquire(myBinarySemuart_usbHandle,50)==osOK){
            usart_printf(&huart1,usb.proc);//printf("%s\r\n",usb.proc);
        }
        if(osSemaphoreAcquire(myBinarySemuart_blueHandle,50)==osOK){
            rx_len = strlen(blue.proc);LED2_TOGGLE();//printf("rx=%s,len=%d\r\n",blue.proc,rx_len);//单字节中断方式，是时域上的不同
            //usart_printf(&huart1,blue.proc);
            ///数字(0~9)=(48~57)````大写字母(A~Z)=(65~90)````小写字母(a~z)=(97~122)
            if(rx_len==1){//单字节数据控制小车
                //if('A'<=blue.proc[0] && blue.proc[0]<='Z'){}
                switch (blue.proc[0]) {//:判断小车行为（刹车|前|后|右|左）//加/减速
                    case 'Y':Target_Velocity-=100,Flag_velocity=2;break;
                    case 'X':Target_Velocity+=100,Flag_velocity=1;break;
                    case 'A':Flag_front=1,Flag_back=Flag_Left=Flag_Right=0;break;
                    case 'B':Flag_front=Flag_Right=1,Flag_back=Flag_Left=0;break;
                    case 'C':Flag_Right=1,Flag_back=Flag_Left=Flag_front=0;break;
                    case 'D':Flag_back=Flag_Left=1,Flag_front=Flag_Right=0;break;
                    case 'E':Flag_back=1,Flag_Right=Flag_Left=Flag_front=0;break;
                    case 'F':Flag_back=Flag_Right=1,Flag_front=Flag_Left=0;break;
                    case 'G':Flag_Left=1,Flag_Right=Flag_back=Flag_front=0;break;
                    case 'H':Flag_front=Flag_Left=1,Flag_back=Flag_Right=0;break;
//AUTO模式
                    case 'a':case 'b':case 'c':case 'd':case 'e':case 'f':case 'g':case 'h':
                        if(app.Control_flag==AUTO){
                            app.state=Line;runBeepAcktion(BEEP_UART);     //提示接收到控制指令
                            app.Number_mode=blue.proc[0]-96;//送药数字(1~8)
                        }break;
                    case 'i':runBeepAcktion(BEEP_UART);     //提示接收到控制指令
                        app.Control_flag=!app.Control_flag;
                        break;//修改模式为-（Auto/Manual）
//Manual模式
                    case 'j':
                        if(app.Control_flag==MANUAL){
                            app.state=L_90;runBeepAcktion(BEEP_LOOP);         //左转
                            osTimerStart(myTimer_carHandle,TURN_90_DELAY);
                        }break;
                    case 'k':
                        if(app.Control_flag==MANUAL){
                            app.state=T_180;runBeepAcktion(BEEP_LOOP);        //掉头
                            osTimerStart(myTimer_carHandle,TURN_90_DELAY*2);
                        }break;
                    case 'l':
                        if(app.Control_flag==MANUAL){
                            app.state=R_90;runBeepAcktion(BEEP_LOOP);        //右转
                            osTimerStart(myTimer_carHandle,TURN_90_DELAY);
                        }break;
                    case 'm':
                        if(app.Control_flag==MANUAL){
                            app.state=Line;runBeepAcktion(BEEP_UART);     //提示接收到控制指令
                        }break;      //巡线/路口/尽头测试
                    default: Flag_front=Flag_back=Flag_Left=Flag_Right=0;//刹车
                }
            }
            else if(blue.proc[0]=='{' && blue.proc[rx_len-1]=='}' && blue.proc[2]==':'){
                if(blue.proc[1]!=0x23){ //确定是帧头帧尾，开始数据提取// {+类型+?+0x50+个+十+....+}
                    int j,i=rx_len-2;float Data;
                    for (j=i ; j>=3; j--) {Data+=(blue.proc[j]-48)*pow(10,i-j);}//合成数据
                    switch (blue.proc[1]) {
                        case '0':  Target_Velocity=Data;break;
                        case '1':  Balance_Kp=Data;break;
                        case '2':  Balance_Kd=Data;break;
                        case '3':  Velocity_Kp=Data;break;
                        case '4':  Velocity_Ki=Data;break;
                        case '5':  Turn_Kp=Data;break;
                        case '6':  Turn_Kd=Data;break;
                        default: break;
                    }Data=0;PID_Send=1;//runBeepAcktion(BEEP_UART);
                }
            }
        }
        if(osSemaphoreAcquire(myBinarySemuart_k210Handle,50)==osOK){
            rx_len = strlen(k210.proc);LED1_TOGGLE();//usart_printf(&huart1,k210.proc);//printf("len=%d,str=%s",rx_len,k210.proc);LED1_TOGGLE();
            if(k210.proc[0]=='[' && k210.proc[2]==',' && k210.proc[rx_len-1]==']'){  //格式正确
                if(k210.proc[1]=='O')balance.Middle_line=(k210.proc[3]-'0')*100+(k210.proc[4]-'0')*10+(k210.proc[5]-'0'); //巡线模式，提取巡线中线值
                //if(k210.proc[1]=='N')balance.Middle_line=112;//printf("%d\r\n",balance.Middle_line);
                if(app.state==Line){
                    if(k210.proc[1]=='N'){//丢失目标，识别为路口尽头
                        runBeepAcktion(BEEP_LOOP);
                        app.state=Corner;osTimerStart(myTimer_carHandle,CORNER_N_DELAY);
                        osEventFlagsSet(myEvent_AppHandle,EVENT_K210_FIND_END);
                        balance.Middle_line=112;app.Turn_mode=180;
                    }else if(k210.proc[1]=='T'){//发现路口
                        runBeepAcktion(BEEP_LOOP);
                        app.state=Cross;osTimerStart(myTimer_carHandle,CROSS_T_DELAY);
                        osEventFlagsSet(myEvent_AppHandle,EVENT_K210_FIND_TURN);
                    }
                }
            }
        }
        //osDelay(20);
    }
}

void UartTx_Task(void *argument)//打印到APP上面
{
    /* USER CODE BEGIN UartTx_Task */
    static u8 flag;char buf[30];
    int Encoder_Left_Show,Encoder_Right_Show,Voltage_Show;
    /* Infinite loop */
    for(;;)
    {
        Voltage_Show=(adc.battery_voltage*100-1110)*2/3;if(Voltage_Show<0)Voltage_Show=0;if(Voltage_Show>100)Voltage_Show=100;   //对电压数据进行处理
        Encoder_Right_Show=balance.Velocity_Right*1.1; if(Encoder_Right_Show<0) Encoder_Right_Show=-Encoder_Right_Show;			  //对编码器数据就行数据处理便于图形化
        Encoder_Left_Show=balance.Velocity_Left*1.1;  if(Encoder_Left_Show<0) Encoder_Left_Show=-Encoder_Left_Show;
        flag=!flag;
        ///发送PID参数,在APP调参界面显示
        if(PID_Send==1)
        {
            sprintf(buf,"{C%d:%d:%d:%d:%d:%d:%d}$",(int)Target_Velocity,(int)Balance_Kp,(int)Balance_Kd,(int)Velocity_Kp,(int)Velocity_Ki,(int)Turn_Kp,(int)Turn_Kd);
            PID_Send=0;
        }
        /// 发送电池电压，速度，角度等参数，在APP首页显示
        else if(flag==0)sprintf(buf,"{A%d:%d:%d:%d}$",(int)Encoder_Left_Show,(int)Encoder_Right_Show,(int)Voltage_Show,(int)mpu6050.Angle_Balance);
        ///x，y，z轴角度 在APP上面显示波形
        else if(flag==1)sprintf(buf,"{B%d:%d:%d}$",(int)Pitch,(int)Roll,(int)Yaw);
        usart_printf(&huart3,buf);//打印到APP上面
        //可按格式自行增加显示波形，最多可显示五个
        osDelay(100);
    }
    /* USER CODE END UartTx_Task */
}


